#include <ros/ros.h>
#include "bt_manager/bt_manager.hpp"
#include "bt_manager/blackboard.hpp"
#include "bt_manager/ros_interface.hpp"

int main(int argc, char** argv)
{
  ros::init(argc, argv, "bt_manager_node");
  ros::NodeHandle nh;
  
  // 创建黑板
  auto blackboard = BT::Blackboard::create();
  
  // 创建ROS接口
  auto ros_interface = std::make_shared<Sentry_BT::RosInterface>(nh, blackboard);
  blackboard->set("ros_interface", ros_interface);
  
  // 初始化行为树管理器
  Sentry_BT::BTManager bt_manager;
  std::string xml_file_path = "path/to/nav_tree.xml";
  
  if(!bt_manager.initialize(xml_file_path, blackboard))
  {
    ROS_ERROR("Failed to initialize behavior tree");
    return -1;
  }
  
  ROS_INFO("Behavior tree initialized successfully");
  
  // 主循环
  ros::Rate rate(10); // 10Hz
  while(ros::ok())
  {
    bt_manager.execute();
    ros::spinOnce();
    rate.sleep();
  }
  
  return 0;
}